#!/usr/bin/env python
import roslib; roslib.load_manifest('object_seeking')
import rospy
from geometry_msgs.msg import Twist
from irobot_create_2_1.msg import SensorPacket
from ar_recog.msg import Tags
from ar_recog.msg import Tag
# global variables 
bump = False
length = 0
t = 0
num = 0
i = 0
tagList = []
count = 0
isFound = False
distance = 0
tagid = 0
pos = 0
firstIteration = True
lastError = 0.0
points = []
# listen (adapted from line_follower
def processSensing(sensorPacket):
    global bump
    bump = sensorPacket.bumpLeft or sensorPacket.bumpRight
    #newInfo = True
def tagSensing(tag):
    global t
    t = tag.id
def tagsSensing(tags):
    global i
    i = 0
    global count 
    count = tags.tag_count
    global length
    length = len(tags.tags)
    global tagList
    tagList = tags.tags
    global tagid
    if count == 0:
        return
    else:
        tagid = tags.tags[i].id
    global isFound
    for t in tagList:
        if tagid == num:
            isFound = True
    global distance
    for i in range(len(tagList)):
        distance = tags.tags[i].distance
    global pos
    if count == 0:
        return
    else:
        pos = tags.tags[i].x
    global top
    if count == 0:
        return
    else:
        top = tagList[i].cwCorners[2] - tagList[i].cwCorners[0]
    global bottom
    if count == 0:
        return
    else:
        bottom = tagList[i].cwCorners[4] - tagList[i].cwCorners[6]
    global left
    if count == 0:
        return
    else:
        left = tagList[i].cwCorners[7] - tagList[i].cwCorners[1]
    global right
    if count == 0:
        return
    else:    
        right = tagList[i].cwCorners[5] - tagList[i].cwCorners[3]
    global points
    if count == 0:  
        return
    else:
        points = tags.tags[i].cwCorners
def object_seeking():
    pub = rospy.Publisher('cmd_vel', Twist)
    rospy.Subscriber('sensorPacket', SensorPacket, processSensing)
    rospy.Subscriber('tag', Tag, tagSensing)
    rospy.Subscriber('tags', Tags, tagsSensing)
    rospy.init_node('object_seeking')
    #listen
    global bump
    global t
    global isFound
    global array
    global length
    global num
    global i
    global distance
    global tagid
    global pos
    global firstIteration
    global lastError
    global points
    tagflag = 0
    width = 640
    error = 0
    turn = 0
    k = 0.5
    kd = 0.5
    ki = 0.05
    dt = 0.5
    integral = 0
    derivative = 0
    twist = Twist()
    twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0

    while not rospy.is_shutdown():
        twist.angular.z = 0.5
        str = "tag id = %s"%tagid
        rospy.loginfo(str)
        str = "looking for = %s"%num
        rospy.loginfo(str)  
        str = "distance = %s"%distance
        rospy.loginfo(str)  
        if isFound:
            lastError = error
            error = ((width/2.0) - (((points[2] - points[0])/2) + points[0]))/width
            integral = integral + (error * dt)
            derivative = (error - lastError)/dt
            str = "FOUND TAG %s"%rospy.get_time()
            rospy.loginfo(str)
            str = "position = %s"%pos
            rospy.loginfo(str)  
            str = "top = %s"%top
            rospy.loginfo(str)  
            str = "bottom = %s"%bottom
            rospy.loginfo(str)  
            str = "left = %s"%left
            rospy.loginfo(str)
            str = "right = %s"%right
            rospy.loginfo(str)
            print points   
            if distance > 250:
                twist.angular.z = (k * error) + (ki * integral) + (kd * derivative)
                twist.linear.x = 0.3 * (0.5 - abs(error))
                lastError = error
            else:
                isFound = False
                num = num + 1
                i = i + 1
                twist.angular.z = 0.5
                twist.linear.x = 0
            rospy.sleep(1.0)
            str = "looking for = %s"%num
            rospy.loginfo(str)  
        else:
            str = "DIDN'T FIND TAG %s"%rospy.get_time()
            rospy.loginfo(str)
            twist.angular.z = 0.5
        pub.publish(twist)
                #top = tagList[i].cwCorners[2] - tagList[i].cwCorners[0]
                #bottom = tagList[i].cwCorners[4] - tagList[i].cwCorners[6]
                #left = tagList[i].cwCorners[1] - tagList[i].cwCorners[7]
                #right = tagList[i].cwCorners[3] - tagList[i].cwCorners[5]
                #setpoint = (tagList[i].cwCorners[2] - tagList[i].cwCorners[0])/2
                
if __name__ == '__main__':
    try:
        object_seeking()
    except rospy.ROSInterruptException: pass
